#include "SpaSolver.h"
#include <open_karto/Karto.h>

SpaSolver::SpaSolver() {}

SpaSolver::~SpaSolver() {}

void SpaSolver::Clear()
{
    corrections.clear();
}

const karto::ScanSolver::IdPoseVector &SpaSolver::GetCorrections() const
{
    return corrections;
}

void SpaSolver::Compute()
{
    corrections.clear();
    typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> > NodeVector;
    printf("DO SPA BEGIN\n");
    m_Spa.doSPA(40);
    printf("DO SPA END\n");
    NodeVector nodes = m_Spa.getNodes();
    forEach(NodeVector, &nodes)
    {
        karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot);
        corrections.push_back(std::make_pair(iter->nodeId, pose));
    }
}

void SpaSolver::AddNode(karto::Vertex <karto::LocalizedRangeScan> *pVertex)
{
    karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
    Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading());
    m_Spa.addNode(vector, pVertex->GetObject()->GetUniqueId());
}

void SpaSolver::AddConstraint(karto::Edge <karto::LocalizedRangeScan> *pEdge)
{
    karto::LocalizedRangeScan *pSource = pEdge->GetSource()->GetObject();
    karto::LocalizedRangeScan *pTarget = pEdge->GetTarget()->GetObject();
    karto::LinkInfo *pLinkInfo = (karto::LinkInfo * )(pEdge->GetLabel());
    karto::Pose2 diff = pLinkInfo->GetPoseDifference();
    Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading());
    karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
    Eigen::Matrix<double, 3, 3> m;
    m(0, 0) = precisionMatrix(0, 0);
    m(0, 1) = m(1, 0) = precisionMatrix(0, 1);
    m(0, 2) = m(2, 0) = precisionMatrix(0, 2);
    m(1, 1) = precisionMatrix(1, 1);
    m(1, 2) = m(2, 1) = precisionMatrix(1, 2);
    m(2, 2) = precisionMatrix(2, 2);
    m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
}
